#include "TLD_Timer.h"
#include <stdio.h>

unsigned int TLD_timer_ticks = 0;


//the speed of the timer when using ClockDivider_1024
#define TIMER_SPEED (BUS_CLOCK/1024/1000)

/*

*/

TLD_timer_task *TLD_timer_head = NULL, *TLD_timer_tail = NULL;

/*
   Function: TLD_timer_start

   Starts the hardware timer 0 with ClockDivider_1024
   
   Example:
   (start code)
   void five_times_a_sec(void){
	printf("Im here every 200ms!";
	TLD_timer_addTask(200, five_times_a_sec());
   }
   
   TLD_timer_start();
   TLD_timer_addTask(200, five_times_a_sec());
   
   while(1){
    TLD_timer_update();
   }
   (end code)
   
   See also:
   <TLD_timer_stop>
*/
void TLD_timer_start(){
	timerStart(0, ClockDivider_1024, 0, NULL);
}


/*
   Function: TLD_timer_pause

   Pauses the hardware timer 0
   
   All pending tasks will be executed later after unpause is called.
   
   See also:
   <TLD_timer_unpause>
*/
void TLD_timer_pause(){
	timerPause(0);
}

/*
   Function: TLD_timer_unpause

   Unpauses the hardware timer 0
   
   All task will continue to wait or execute during update
   
   See also:
   <TLD_timer_pause>
*/
void TLD_timer_unpause(){
	timerUnpause(0);
}

/*
   Function: TLD_timer_stop

   Stops the hardware timer 0
   Removes the current waiting tasks
   
   See also:
   <TLD_timer_removeTask>
*/
void TLD_timer_stop(){
	timerStop(0);

	TLD_timer_task *t = TLD_timer_head;
	while(t != NULL){
		TLD_timer_task *next = t->next;
		TLD_timer_removeTask(t);
		t = next;
	}
}

/*
   Function: TLD_timer_addTask

   Adds a new task to the linked tasks list
   
   Parameters:
   ms - The time to wait for execution
   f - The to be executed function
   
   See also:
   <TLD_timer_task_action>
   
   <TLD_timer_removeTask>
*/
void TLD_timer_addTask(int ms, TLD_timer_task_action f){
	TLD_timer_ticks += timerElapsed(0);
	unsigned int msecs_passed = TLD_timer_ticks / TIMER_SPEED;

	TLD_timer_task *p = (TLD_timer_task*) malloc( sizeof( TLD_timer_task ));
	p->msecs = msecs_passed + ms;
	p->f = f;

	// Append node
	if(TLD_timer_head == NULL){
		TLD_timer_head = p;
		p->prev = NULL;
	}else{
		TLD_timer_tail->next = p;
		p->prev = TLD_timer_tail;
	}
	TLD_timer_tail = p;
	p->next = NULL;
}


/*
   Function: TLD_timer_removeTask

   Removes a task from the list.
   
   Parameters:
   *t - The task to be removed
   
   See also:
   <TLD_timer_task>
   
   <TLD_timer_addTask>
*/
void TLD_timer_removeTask(TLD_timer_task *t){
	if(t->prev == NULL){
		TLD_timer_head = t->next;
	}else{
		t->prev->next = t->next;
	}

	if(t->next == NULL){
		TLD_timer_tail = t->prev;
	}else{
		t->next->prev = t->prev;
	}

	free(t);  /* We are done with the struct, and
	               free the associated pointer. */
}

/*
   Function: TLD_timer_update
   
   Updates the internal timer ticks.
   Calculates the difference between now and execution time for each task.
   Executes the task when the time has come :D
   
   *Only works when <TLD_timer_start> is called first!*
   Could be handy to have tasks.
*/
void TLD_timer_update() {
	TLD_timer_ticks += timerElapsed(0);
	unsigned int msecs_passed = TLD_timer_ticks / TIMER_SPEED;

	TLD_timer_task *t;
	t = TLD_timer_head;
	while(t != NULL){
		TLD_timer_task *next = t->next;
		if (msecs_passed >= t->msecs) {
			t->f();

			TLD_timer_removeTask(t);
		}
		t = next;
	}
}


